#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp>
#include <geographic_msgs/msg/geo_point_stamped.hpp>
#include <geometry_msgs/msg/vector3_stamped.hpp>
#include <std_msgs/msg/float64.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <vector>
#include <map>
#include <thread>
#include <mutex>
#include <cmath>
#include <sstream>
#include <iostream>
#include <netinet/in.h>
#include <sys/socket.h>
#include <cstring>

#include "cpp_co_location_client/global_functions.hpp"

#include "client_interfaces/msg/estimate_target_position.hpp"

constexpr double EARTH_RADIUS = 6378137.0; // 地球半径（单位：米）

class MultiDroneTargetProcessor : public rclcpp::Node {
public:
    MultiDroneTargetProcessor()
        : Node("multi_drone_target_processor") {
        this->declare_parameter<int>("udp_port", 9999);
        this->declare_parameter<std::vector<std::string>>("drone_names", {"drone_0", "drone_1", "drone_2"});
        this->get_parameter("udp_port", udp_port_);
        this->get_parameter("drone_names", drone_names_);

        for (const auto &drone_name : drone_names_) {
            DroneState state;
            state.lat = 0.0;
            state.lon = 0.0;
            state.alt = 0.0;
            state.ned_x = 0.0;
            state.ned_y = 0.0;
            state.ned_z = 0.0;
            state.yaw = 0.0;
            state.target_local_x = 0.0;
            state.target_local_y = 0.0;
            state.target_local_z = 0.0;
            state.target_position = {0,0,0};
            drone_states_[drone_name] = state;

            // // 订阅无人机 Odom 位置信息和偏航角信息
            // std::string odom_position_topic = drone_name + "/local_position/odom";
            // auto odom_sub = this->create_subscription<geometry_msgs::msg::Vector3Stamped>(
            //     odom_position_topic, 10,
            //     [this, drone_name](geometry_msgs::msg::Vector3Stamped::SharedPtr msg) {
            //         std::lock_guard<std::mutex> lock(state_mutex_);
            //         drone_states_[drone_name].ned_x = msg->vector.x;
            //         drone_states_[drone_name].ned_y = msg->vector.y;
            //         drone_states_[drone_name].ned_z = msg->vector.z;
            //     });
            // ned_position_subs_.push_back(odom_sub);

            std::string yaw_topic = drone_name + "/global_position/compass_hdg";
            auto yaw_sub = this->create_subscription<std_msgs::msg::Float64>(
                yaw_topic, 10,
                [this, drone_name](std_msgs::msg::Float64::SharedPtr msg) {
                    std::lock_guard<std::mutex> lock(state_mutex_);
                    double yaw = msg->data * M_PI / 180.0; // 转为弧度
                    drone_states_[drone_name].yaw = M_PI/2.0 - yaw;// ENU 下的 yaw
                });
            yaw_subs_.push_back(yaw_sub);

            // 订阅无人机经纬度
            std::string global_position_topic = drone_name + "/global_position/global";
            auto global_sub = this->create_subscription<sensor_msgs::msg::NavSatFix>(
                global_position_topic, 10,
                [this, drone_name](sensor_msgs::msg::NavSatFix::SharedPtr msg) {
                    std::lock_guard<std::mutex> lock(state_mutex_);
                    drone_states_[drone_name].lat = msg->latitude;
                    drone_states_[drone_name].lon = msg->longitude;
                    drone_states_[drone_name].alt = msg->altitude;
                });
            global_position_subs_.push_back(global_sub);
        }

        // 创建 UDP 接收线程
        udp_thread_ = std::thread(&MultiDroneTargetProcessor::udp_receive_loop, this);

        // 发布融合的目标位置
        fused_target_pub_ = this->create_publisher<client_interfaces::msg::EstimateTargetPosition>("estimate_target_position", 10);
    }

    ~MultiDroneTargetProcessor() {
        if (udp_thread_.joinable()) {
            udp_thread_.join();
        }
    }

private:
    struct GeoPoint {
        double latitude;
        double longitude;
        double altitude;
    };

    struct DroneState {
        double lat, lon, alt;    // 经纬高
        double ned_x, ned_y, ned_z; // NED 坐标
        double yaw;              // 偏航角（弧度）
        double target_local_x, target_local_y, target_local_z; // UDP 接收到的机体相对坐标
        GeoPoint target_position; // 融合目标位置
    };

    int udp_port_;
    std::vector<std::string> drone_names_;
    std::map<std::string, DroneState> drone_states_;
    std::vector<rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr> global_position_subs_;
    // std::vector<rclcpp::Subscription<geometry_msgs::msg::Vector3Stamped>::SharedPtr> ned_position_subs_;
    std::vector<rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr> yaw_subs_;
    std::thread udp_thread_;
    rclcpp::Publisher<client_interfaces::msg::EstimateTargetPosition>::SharedPtr fused_target_pub_;
    std::mutex state_mutex_;

    void udp_receive_loop() {
        int udp_socket = socket(AF_INET, SOCK_DGRAM, 0);
        if (udp_socket < 0) {
            throw std::runtime_error("Failed to create UDP socket");
        }

        sockaddr_in server_addr{};
        server_addr.sin_family = AF_INET;
        server_addr.sin_addr.s_addr = INADDR_ANY;
        server_addr.sin_port = htons(udp_port_);

        if (bind(udp_socket, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0) {
            throw std::runtime_error("Failed to bind UDP socket");
        }

        char buffer[1024];
        sockaddr_in client_addr{};
        socklen_t addr_len = sizeof(client_addr);

        while (rclcpp::ok()) {
            ssize_t len = recvfrom(udp_socket, buffer, sizeof(buffer), 0, (struct sockaddr *)&client_addr, &addr_len);
            if (len > 0) {
                buffer[len] = '\0';
                process_udp_message(std::string(buffer));
            }
        }

        close(udp_socket);
    }

    void process_udp_message(const std::string &message) {
        std::istringstream iss(message);
        std::string drone_id_str, token;
        // double target_local_x, target_local_y, target_local_z;
        char delimiter;

        std::vector<double> numbers;

        while(std::getline(iss,token,',')){
            numbers.push_back(std::stoi(token));
        }

        drone_id_str = std::to_string(numbers[0]);

        std::string drone_name = "drone_" + drone_id_str;
        std::lock_guard<std::mutex> lock(state_mutex_);
        if (drone_states_.count(drone_name) == 0 ) {
            RCLCPP_WARN(this->get_logger(), "Unknown drone ID: %s", drone_name.c_str());
            return;
        }

        if (numbers.size()<4) {
            RCLCPP_WARN(this->get_logger(), "Number size is not sufficient!");
            return;
        }

        auto &state = drone_states_[drone_name];
        state.target_local_x = numbers[1];
        state.target_local_y = numbers[2];
        state.target_local_z = -numbers[3];

        // 机体坐标系 -> ENU 坐标系
        double target_ned_x = std::cos(state.yaw) * state.target_local_x - std::sin(state.yaw) * state.target_local_y;
        double target_ned_y = std::sin(state.yaw) * state.target_local_x + std::cos(state.yaw) * state.target_local_y;
        double target_ned_z = state.target_local_z;

        enu_to_wgs84(target_ned_x, target_ned_y, target_ned_z, 
                  state.lat, state.lon, state.alt,
                  state.target_position.latitude, state.target_position.longitude, state.target_position.altitude);

        GeoPoint fused_target_position;
        fused_target_position.latitude = 0.0;
        fused_target_position.longitude = 0.0;
        fused_target_position.altitude = 0.0;

        for(auto drone : drone_states_){
            fused_target_position.latitude += drone.second.target_position.latitude;
            fused_target_position.longitude += drone.second.target_position.longitude;
            fused_target_position.altitude += drone.second.target_position.altitude;
        }
        fused_target_position.latitude /= drone_states_.size();
        fused_target_position.longitude /= drone_states_.size();
        fused_target_position.altitude /= drone_states_.size();

        // 发布融合结果
        client_interfaces::msg::EstimateTargetPosition fused_msg;
        fused_msg.header.stamp = this->now();
        fused_msg.header.frame_id = "map";
        fused_msg.position.latitude = fused_target_position.latitude ;
        fused_msg.position.longitude = fused_target_position.longitude;
        fused_msg.position.altitude = fused_target_position.altitude;
        fused_target_pub_->publish(fused_msg);

        RCLCPP_INFO(this->get_logger(), "Drone: %s, Fused Target Position: [%.8f, %.8f, %.2f]",
                    drone_name.c_str(), fused_target_position.latitude , fused_target_position.longitude, fused_target_position.altitude);
    }
};

int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MultiDroneTargetProcessor>());
    rclcpp::shutdown();
    return 0;
}
